// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CameraPoseEstimation.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.ImageProcessing2D
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.GroundTruthExtraction;
    
    /// <summary>
    /// Class that computes the 3D pose of a camera given that it is viewing a checkerboard
    /// </summary>
    public sealed class CameraPoseEstimation : IDisposable
    {
        /// <summary>
        /// Estimates the 3D pose (despite the class name) of the camera
        /// </summary>
        private ManagedPlanarPoseEstimator poseEstimator = null;

        /// <summary>
        /// Gets the camera intrinsics
        /// </summary>
        private Matrix3 intrinsics;

        /// <summary>
        /// Gets the distortion parameters
        /// </summary>
        private LensDistortion distortionParameters;

        /// <summary>
        /// Initializes a new instance of the CameraPoseEstimation class
        /// </summary>
        /// <param name="intrinsics">The intrinsics of the camera</param>
        /// <param name="lensDistortion">The lens distortion</param>
        public CameraPoseEstimation(
            Matrix3 intrinsics,
            LensDistortion lensDistortion)
        {
            this.intrinsics = intrinsics;
            this.distortionParameters = lensDistortion;
            this.SetParameters(intrinsics, lensDistortion);
        }

        /// <summary>
        /// Estimate the pose of the camera
        /// </summary>
        /// <param name="rgbImage">The color image</param>
        /// <param name="intrinsics">The camera intrinsics</param>
        /// <param name="distortion">The distortion parameters</param>
        /// <returns>A tuple containing the pose and a value indicating if the checkerboard was seen.</returns>
        public Tuple<Pose, bool> EstimatePose(ImageFrameRgb rgbImage, Matrix3 intrinsics, LensDistortion distortion)
        {
            ImageFrameGray8 grayImage = new ImageFrameGray8(rgbImage.Width, rgbImage.Height);

            // conver to gray image
            for (int i = 0; i < rgbImage.ImageData.Length; i++)
            {
                grayImage[i] = Convert.ToByte(Math.Min(255, ((int)rgbImage[i].Red + (int)rgbImage[i].Green + (int)rgbImage[i].Blue) / 3));
            }

            return this.EstimatePose(grayImage, intrinsics, distortion);
        }

        /// <summary>
        /// Estimate the pose of the camera
        /// </summary>
        /// <param name="grayImage">The gray image</param>
        /// <param name="intrinsics">The camera intrinsics</param>
        /// <param name="distortion">The distortion parameters</param>
        /// <returns>A tuple containing the pose and a value indicating if the checkerboard was seen.</returns>
        public Tuple<Pose, bool> EstimatePose(ImageFrameGray8 grayImage, Matrix3 intrinsics, LensDistortion distortion)
        {
            if (intrinsics != this.intrinsics || this.distortionParameters != distortion)
            {
                this.SetParameters(intrinsics, distortion);
            }

            double[] translation = new double[3];
            double[] quaternion = new double[4];
            bool checkerBoardFound = false;

            unsafe
            {
                fixed (double* trans = &(translation[0]))
                {
                    fixed (double* quat = &(quaternion[0]))
                    {
                        checkerBoardFound = this.poseEstimator.EstimatePoseWithRotationAsUnitQuaternion(grayImage, trans, (uint)3, quat, (uint)4);
                    }
                }
            }

            return new Tuple<Pose, bool>(
                new Pose(
                    new Vector3(translation[0], translation[1], translation[2]),
                    new Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])),
                checkerBoardFound);
        }

        /// <summary>
        /// Disposes the object
        /// </summary>
        public void Dispose()
        {
            this.poseEstimator.Dispose();
        }

        /// <summary>
        /// Sets the parameters for the estimator with the camera parameters
        /// </summary>
        /// <param name="intrinsics">The intrinsic parameters</param>
        /// <param name="distortion">The distortion parameters</param>
        private void SetParameters(
            Matrix3 intrinsics,
            LensDistortion distortion)
        {
            // Bug: Task 2223.  Add lens distortion parameters
            double[] distortionParameters = new double[] { 0, 0, 0, 0, 0 };
            double[] cameraParameters = new double[] 
                {  
                    intrinsics[0, 0],
                    intrinsics[0, 1],
                    intrinsics[1, 1],
                    intrinsics[0, 2],
                    intrinsics[1, 2] 
                };

            unsafe
            {
                fixed (double* cameraParams = &(cameraParameters[0]))
                {
                    fixed (double* distParams = &(distortionParameters[0]))
                    {
                        this.poseEstimator = new ManagedPlanarPoseEstimator(cameraParams, 5, distParams, 5);
                    }
                }
            }
        }
    }
}
